/*============================================================================*/
/*                                                                            */
/* utils.c                                                                    */
/*                                                                            */
/* This file contains the following utilities.                                */
/* - loading/saving the list of corners                                       */
/* - evaluating the result of corner detection                                */
/* - loading image from a TGA file                                            */
/*                                                                            */
/* (c) Eduard Sojka, 2002                                                     */
/*                                                                            */
/*============================================================================*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#define  MAX_DIST 4                  /* maximal allowed distance between true */
                                     /* and detected position of a corner     */
/*============================================================================*/
/* Reading/saving the list of corners from/on a file.                         */

# define LINE_LENGTH 256

int loadCornerList(char *fileName, int *corners, int arraySize){
    FILE *inpFile; int k, l, x, y, cnt = 0; 
    char line[LINE_LENGTH], atom[LINE_LENGTH];
    if((inpFile = fopen( fileName, "rt" )) != NULL){
      while (fgets(line, LINE_LENGTH, inpFile) != NULL)
             if (strlen(line) > 1)
               if (cnt < arraySize - 1){
                 k = 0; while (line[k] == 32) k++;
                 l = 0; while (line[k] > 32) atom[l++] = line[k++];
                 atom[l] = 0; x = atoi(atom);
                 corners[cnt++] = (int)(x/1000);
                 while (line[k] == 32) k++;
                 l = 0; while (line[k] > 32) atom[l++] = line[k++];
                 atom[l] = 0; y = atoi(atom);
                 corners[cnt++] = (int)(y/1000);
             } else { fclose(inpFile);  return -2;}
      fclose(inpFile); return cnt / 2;
    } else return -1;
}

int saveCornerList(char *fileName, int *corners, int numCorners){
    FILE *outputFile; int i;
    outputFile = fopen(fileName,"w");
    if (outputFile != NULL){
      for (i = 0; i < 2*numCorners; i++,i++ )
        fprintf(outputFile, "%i %i\n", corners[i]*1000, corners[i+1]*1000);
      fclose(outputFile);
      return numCorners;
    } else return -1;
}
/*============================================================================*/
/* Functions fo evaluating the detection                                      */

float distance(int x1, int y1, int x2, int y2){
    int dx = x2 - x1, dy = y2 - y1;
    return sqrt((float)(dx*dx + dy*dy));
}

int findNearest(int *list, int listSize, int x, int y, float *dist){
    int i,minDistIdx; float minDist,d;
    if (listSize > 0){
       minDist = distance(list[0],list[1], x,y); minDistIdx = 0;
       for (i = 2; i < listSize; i++, i++ ){
         d = distance(list[i], list[i+1], x, y);
         if (d < minDist){minDist = d; minDistIdx = i;}
       }
       *dist = minDist; return minDistIdx;
    } else return -1;
}

#define removeFromActSol( k ) \
tmp = actSol[k]; actSol[k] = actSol[--actSolLen]; actSol[actSolLen] = tmp;

int evaluateDetection( int *actSol, int actSolLen, int *refSol, int refSolLen,
                       int *numFound, int *numMissed, int *numFalse,
                       int *numMmultiple, int *totErr,  float *locErr){

    int i, idx, rep, tmp, found = 0, multiple = 0;
    float dist, sumDists = 0.0f;
    actSolLen *= 2; refSolLen *= 2;
    for (i = 0; i < refSolLen; i++, i++){
      idx = findNearest( actSol, actSolLen, refSol[i], refSol[i+1], &dist );
      if (idx >= 0)
        if (dist <= MAX_DIST){
            sumDists += dist; found++;
            removeFromActSol(idx+1);
            removeFromActSol(idx);
        }
    };

    for (i = 0; i < refSolLen; i++,i++)
      do { idx = findNearest(actSol, actSolLen, refSol[i], refSol[i+1], &dist );
           if (idx >= 0){
             if (dist <= MAX_DIST){
               rep = 1; multiple++;
               removeFromActSol(idx+1);
               removeFromActSol(idx);
             } else rep = 0;
           } else rep = 0;
      } while (rep == 1);

   *numFound     = found;
   *numMissed    = refSolLen/2 - found;
   *numFalse     = actSolLen/2;
   *numMmultiple = multiple;
   *totErr       = *numMissed + *numFalse + *numMmultiple;
   *locErr       = sumDists / found;
    return *totErr;
}
/*============================================================================*/
/* A function for reading the gray-scale image whose name is in fileName.     */
/* Returns the pointer to the image as a function value, and the sizes        */
/* of the image in xSize, ySize. In case of any failure, NULL is returned.    */

unsigned char *loadImageFromTGA(char *fileName, int *xSize, int *ySize){
   FILE *file;
   unsigned char *image, header[18], id_field_len, image_type, pixel_size,
                  image_descriptor;
   int            width, height, y, idx, lowerLeft;

   file = fopen(fileName,"rb"); if (!file) return NULL;
   fread(&header, 1, sizeof(header), file);

   id_field_len     = header[ 0];
   image_type       = header[ 2];
   pixel_size       = header[16];
   image_descriptor = header[17];
   width  = (int)(256*header[13] +  header[12]);
   height = (int)(256*header[15] +  header[14]);

   if ((image_type !=3)||(pixel_size != 8)) return NULL;
   image = (unsigned char *)malloc(width*height); if (image == NULL) return NULL;
   if (id_field_len > 0) fseek(file, id_field_len, SEEK_CUR);
   lowerLeft = (image_descriptor & 0x20) != 0x20;
   idx = 0;
   for (y = 0; y < height; y++) { /* process all lines */
     if (lowerLeft) idx = (height - y - 1)*width; else idx += width;
     fread(image + idx, width, 1, file);
   }
   fclose(file);
   *xSize = width; *ySize = height;
   //printf("x:%d y:%d", width, height);
   return image;
}
/*============================================================================*/
/* Reads next line from inpFile and returns the first atom on this line.      */

int readAtom(FILE *inpFile, char *atom){
  char line[LINE_LENGTH]; int k,l;
  if (fgets(line, LINE_LENGTH, inpFile) != NULL){
    k = 0; while (line[k] == 32) k++;
    l = 0; while (line[k] > 32) atom[l++] = line[k++];
    atom[l] = 0; return 0;
  } else return -1;
}

/*============================================================================*/


